clear all
% close all

springN = 1;

fprintf('\nStiffness script START\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
log_name = sprintf('./spring%d/logs/test',springN);
tc = 0.18;
gr = 17.25;
A2T = tc*gr;


n = 11;

% leg_angle		= [];
% trans_angle	= []; 
% motor_current	= [];

t_start = 1.5;
t_end	= 1.7;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Run controllers


trq = linspace(15,-15,n);

for test_idx = 1:n
	
	fprintf('Running controller:\n');
	fprintf('Command Torque: %f\n',trq(test_idx));
	%			controller	log_file	time_start	time_stop	[controller_parms]
	cmd_str = sprintf('sudo ./bin/fca_interface_headless 1 %s.%d.log %.5f %.5f %.5f > /dev/null',log_name,test_idx, t_start, t_end, trq(test_idx));
	
	ret = unix(cmd_str);
	if (ret ~= 0)
		fprintf('Controller fail!\n');
		return;
	end
	
	pause(0.2);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compile logs

fprintf('Compiling logs\n');

for test_idx = 1:n
	fprintf('Opening log:\t');
	fprintf('%s.%d.log\n',log_name,test_idx);
	
	log_str = sprintf('%s.%d.log',log_name,test_idx);
	log = importdata(log_str);
		
	motor_angle(:,test_idx)		= log.data(:,4);
	leg_angle(:,test_idx)		= log.data(:,5);
	
	motor_current(:,test_idx)	= log.data(:,8);
end

trq = mean(motor_current*A2T);

tmp = polyfit(trq,mean(motor_angle - leg_angle),1);
%off = -tmp(2);

ml = mean(leg_angle);
mm = mean(motor_angle);
off = (ml((n+1)/2) - mm((n+1)/2));

dtheta = mm - ml + off;
tmp = polyfit(trq,dtheta,1);

k_est	= round(1/tmp(1));

fprintf('\n\nK = %f,  offset = %f\n',k_est,off)


% figure
% hold on
% % plot(motor_angle+off,'b','linewidth',2);
% plot(leg_angle,'k','linewidth',2);



%h = figure('Name',['Spring ' int2str(springN)]);
% str = sprintf('Spring %d: Forcing Function',springN);
% title(str);
h = figure;
set(h,'position',[ 100 100 400 400])
hold on

plot([trq(1) trq(end)], [0 0],'k:','linewidth',1);
plot([0 0], [min(trq*tmp(1)) max(trq*tmp(1))],'k:','linewidth',1);

pm = plot(trq, mean(motor_angle - leg_angle+off),'b','linewidth',3);
pa = plot(trq, trq*(1/k_est),'r--','linewidth',2);
% pa = plot(trq, trq*-1/1400,'r--','linewidth',2);


legend([pa;pm],'Aproximation', 'Measured','location','northeast');
axis tight

xlabel(	'\textbf{Torque} $\left( N \hspace{1 mm}\cdot m \hspace{1 mm}\right)$',...
		'interpreter','latex','fontsize',16);

ylabel(	'\textbf{Spring Position} (rad )',...
		'interpreter','latex','fontsize',16);

set(gca,'Position',[ 0.2 0.15 0.75 0.8])
	
str = sprintf('/home/drl/projects/jhurst/current/Kemper_journal/images/forcefunc%d.pdf', springN);
% save2pdf(str);

